// Tests using serial port + XBee to communicate between cubes

#include <iostream>
#include <wiringSerial.h>
#include <sys/types.h>		// For POSIX datatypes
#include <stdint.h>		// For POSIX datatypes
#include <openssl/md4.h>	// For hashing
#include <string.h>
// Comment
#define BAUD_RATE 57600 
#define SER_PORT "/dev/ttyUSB0"
#define DIGEST_SIZE 16
#define MAX_MSG_SIZE 0xFF	// Max size of a message
#define CUBE_ID 1		// ID assigned to this cube
#define CALL_CARD 0x55		// ID of a message
#define HEADER_SIZE 5

typedef struct xMsg{
	uint8_t callCard;	// Signifies start of a message
	uint8_t toAddr;		// Address of sender
	uint8_t fromAddr;	// Address of receiver
	uint8_t size;		// Size of message body
	uint8_t confirmation;	// Used like boolean
	//uint8_t addrSizeDigest[DIGEST_SIZE];	// Digest of address and size header CURRENTLY UNIMPLEMENTED
	uint8_t messageBody[MAX_MSG_SIZE];	// Body of the message 
	uint8_t messageBodyDigest[DIGEST_SIZE];	// Digest of the message body
} xMsg;


xMsg * createMessage(uint8_t to, uint8_t from, uint8_t size, uint8_t * data)
{
	xMsg * tempMsg = new xMsg; 
	tempMsg->callCard = CALL_CARD; 
	tempMsg->toAddr = to; 
	tempMsg->fromAddr = from; 
	tempMsg->size = size; 		
	tempMsg->confirmation = 0x00;				// 0 is not a confirmation. 0xAA is confirmation
	memset(tempMsg->messageBody, 0x00, MAX_MSG_SIZE); 	// Sets all memory body to 0
	memcpy(tempMsg->messageBody, data, size);		// Copies data into message body
	MD4(data, MAX_MSG_SIZE,  tempMsg->messageBodyDigest);	// Creates digest and puts it into struct

	return tempMsg; 
}


int main(int argc, char ** argv)
{

	uint8_t dataBuffer[MAX_MSG_SIZE];
	uint8_t msgDataDigest[DIGEST_SIZE]; 
	uint8_t localDataDigest[DIGEST_SIZE]; 
	uint8_t fromAddr; 
	uint8_t msgSize; 
	uint8_t confirmation; 	
	uint8_t tempHeaderBuff[HEADER_SIZE]; 
	uint8_t remoteHeaderDigest[DIGEST_SIZE]; 
	uint8_t localHeaderDigest[DIGEST_SIZE]; 
	uint8_t localDataBuffer[MAX_MSG_SIZE]; 
	for(int i=0; i<MAX_MSG_SIZE; i++)
		localDataBuffer[i] = (uint8_t)i; 
	memset(localDataDigest, 0x00, MAX_MSG_SIZE); 
	MD4(localDataBuffer, MAX_MSG_SIZE, localDataDigest); 


	int fd = serialOpen(SER_PORT, BAUD_RATE); 
	if(fd >= 0){
		while(1)
		{
			//std::cout << "main loop" << std::endl; 
			if(serialDataAvail(fd) > 0)
			{
				//std::cout << "Data Avail" << std::endl; 
				if(serialGetchar(fd) == CALL_CARD)
				{
					std::cout << "CALL CARD" << std::endl; 
					uint8_t ID = serialGetchar(fd); 
					if(ID == CUBE_ID)
					{
						fromAddr = serialGetchar(fd);		// Gets from addr
						msgSize = serialGetchar(fd); 		// Gets size
						confirmation = serialGetchar(fd); 	// Gets confirmation

						// Fetch header digest
						for(int i=0; i<DIGEST_SIZE; i++){
							remoteHeaderDigest[i] = serialGetchar(fd);
							//std::cout << "msg Digest " << i << "\t" << (unsigned int)msgDataDigest[i] << std::endl; 
						}
						uint8_t tempCall = CALL_CARD;	
						uint8_t cubeID = CUBE_ID; 
						memcpy(tempHeaderBuff, &tempCall, 1); 
						memcpy(tempHeaderBuff+1, &cubeID, 1); 
						memcpy(tempHeaderBuff+2, &fromAddr, 1); 
						memcpy(tempHeaderBuff+3, &msgSize, 1);
						memcpy(tempHeaderBuff+4, &confirmation, 1); 
						MD4(tempHeaderBuff, HEADER_SIZE, localHeaderDigest); 
						
						
						int res1 = memcmp(localHeaderDigest, remoteHeaderDigest, DIGEST_SIZE); 
						std::cout << "Comparison Result: " << res1 << std::endl;
						if(res1 == 0){
							std::cout << "SUCCESS!  Message: " << std::endl; 
							
							// send confirmation
						}else{
							// Send error
							std::cout << "ERROR!" << std::endl; 
						}
						
						std::cout << "FETCH" << std::endl; 
						// Fetch data
						for(int i=0; i<MAX_MSG_SIZE; i++){
							std::cout << i << ":	"; 
							dataBuffer[i] = serialGetchar(fd); 
							//std::cout << (unsigned int)dataBuffer[i] << std::endl; 
						}
						// Fetch data digest
						for(int i=0; i<DIGEST_SIZE; i++){
							msgDataDigest[i] = serialGetchar(fd);
							//std::cout << "msg Digest " << i << "\t" << (unsigned int)msgDataDigest[i] << std::endl; 
						}	
						// Generate digest from msg data	
						MD4(dataBuffer, MAX_MSG_SIZE, localDataDigest); 
						for(int i=0; i<DIGEST_SIZE; i++)
							std::cout << "Local Digest " << i << "\t" << (unsigned int)localDataDigest[i] << std::endl; 
						// Compare the digests
						int res = memcmp(msgDataDigest, localDataDigest, DIGEST_SIZE); 
						std::cout << "Comparison Result: " << res << std::endl;
						if(res == 0){
							std::cout << "SUCCESS!  Message: " << std::endl; 
							
							// send confirmation
						}else{
							// Send error
							std::cout << "ERROR!" << std::endl; 
						}
					}else
						std::cout << "Wrong ID: " << ID <<  std::endl; 
					
				}
			}
		}
	}else{
		std::cout << "Error Opening USB" << std::endl; 	
	}
	
	
	return 0; 
}
